/*! \file cartpole.cpp

\Bat system model: free-floating batbody + wings
Date of creation: 2010-15-11

Copyright
Julian David Colorado, jd.colorado@upm.es
Universidad Politecnica de Madrid

See the file "license.terms" for information on usage and redistribution of this file, and for a
DISCLAIMER OF ALL WARRANTIES.

*/

/*
bat system Model

INPUT
\param MAT SimParam: Simulation Parameters 4 x 6
\param VEC X: State vector 4
\param double TAU: Torque input

OUTPUT

\param VEC out: State modified.
*/

#include "../../dynlib.h"

void dynmodel(Matrix &SimParam,Matrix &X,int i,double Tau,ColumnVector &out)
{

	/*Newton model of bat
	bat(Sim Param,theta,dtheta,d2theta,i,Tau)
	
	M=SimParam->me[1,1]
	m=SimParam->me[1,2]
	L=SimParam->me[1,3]
	
	K=SimParam->me[1,5]
	p=SimParam->me[1,6]
	
	dX->me[0][i]=dx
	dX->me[1][i]=dtheta
	dX->me[2][i]=d2x
	dX->me[3][i]=d2theta
	
	X->ve[0,i]=x
	X->ve[1,i]=theta
	X->ve[2,i]=dx
	X->ve[3,i]=dtheta
	grav=SimParam[2,4]*/
	
	
	double Lm, Mass, St1, Ct1, dt2;
	
	//L*m
	Lm=SimParam(1,3)*SimParam(1,2);

	//M+m
	Mass=SimParam(1,1)+SimParam(1,2);
	
	St1=sin(X(2,i));
	Ct1=cos(X(2,i));
	
	dt2=X(4,i)*X(4,i);
	
	out(1)=X(3,i);
	out(2)=X(4,i);
	
	//L*m*g*sin(theta[1,i])*(M+m)-p*dtheta[1,i]*(M+m)=(L*m*g*sin(theta[1,i])-p*dtheta[1,i])(M+m)
	out(4)=(SimParam(3,5)*St1-SimParam(1,6)*X(4,i)/Lm)*(Mass);
	
	//-L*m*cos(theta[1,i])(K*dtheta[0,i]+Tau-Lm*dtheta[1][i]**2*sin(theta[1][i]))
	out(4)=out(4)-(SimParam(1,5)*X(3,i)+Tau-Lm*dt2*St1)*Ct1;
	
	//d2theta[1][i]=d2theta[1][i]/((M+m)*(I+L**2*m)-L**2*m**2*cos(theta[1][i])**2)
	out(4)=out(4)/(Mass*(13.0/12.0)*SimParam(1,3)+Lm*Ct1*Ct1);
	
	//d2X
	//d2theta[0][i]=(1/(M+m))*(Tau-L*m*(d2theta[1][i])*cos(theta[1][i])-dtheta[1][i]**2*sin(theta[1][i]))-K*dtheta[0][i] 
	out(3)=(1/(Mass))*(Tau-Lm*((out(4))*Ct1-dt2*St1)-SimParam(1,5)*X(3,i));
	
}
